/*----------------------------------------------------------------------------------------------------
** Created by        : [zhaoyang]
** Created date      : 2022-08-26
** Version           : 1.0
** Descriptions      : 距离算法处理
**---------------------------------------------------------------------------------------------------*/


#include <stdbool.h>
#include <stdint.h>
#include "nrf_log.h"
#include "ble_nus.h"
#include "data_handle.h"
#include "nrf_gpio.h"
#include <math.h>
#include "algo_distance.h"
#include "m_data_handle.h"
#include "player.h"

typedef struct
{
    double lat;
    double lon;
}test_gps_t;


#define M_PI            3.14159265358979323846
#define EARTH_RADIUS    6371000        //地球近似半径

#define SSVT_A          4096            //三轴重力加速度1g对应ACC值
#define ONE_G           9.807           //G值
#define GPS_buf_len     2              //gps采样数据buf大小


#define ACC_DATA_LEN_MAX        60      //ACC采样数据总长度；保持最近3个周期的数据
//#define ACC_DATA_GROUP_MAX      3       //ACC采样数据组数，一个周期的数据为1组；保留3组用于算法计算

#define PEAKS_VALLEY_MAX        15      //最大波峰波谷数量，3组的数据，最多理应不会超过15
#define thd                     5       //分析计算波峰/波谷的数据阈值

#define STEP_FREQUENCY_MAX      240     //步频上限值
#define STEP_AMPLITUDE_MAX      2.5     //步幅上限值
#define STEP_HEIGHT_MAX         0.3     //垂直高度上限值
#define HEIGHT_STEP_RATIO       0.15    //垂直步幅比上限
#define SPEED_MAX               12      //速度上限值

#define INDEX_BETWEEN_MIN       4       //5Hz
#define INDEX_BETWEEN_MAX       40      //0.5Hz

#define RTC_CNT_PER_SEC         32768   //每秒对应RTC


#pragma pack(1)

typedef struct
{
    double A;
    double H;
    double Q;
    double R;
    double x;
    double P;

    bool   first;
}Kalman_Filter_Para_t; //Kalman Filter parameter

#pragma pack()


static Kalman_Filter_Para_t kfp_lat = {0};  //纬度使用
static Kalman_Filter_Para_t kfp_lon = {0};  //经度使用

static double g_GPS_lat[GPS_buf_len + 2] = {0};  //GPS纬度数据
static double g_GPS_lon[GPS_buf_len + 2] = {0};  //GPS经度数据
static uint8_t g_GPS_data_cnt       = 0;         //GPS经度数据组数

uint16_t g_step_total       = 0;    //步数
uint16_t g_cal_total        = 0;    //cal

double g_height_total       = 0;    //累计爬升高度
double g_distance_total     = 0;    //根据经纬度计算出的距离，单位米
double g_distance_last      = 0;    //记录上次的距离数据

uint16_t g_step_freq_avg    = 0;    //平均步频
uint16_t step_freq_rt       = 0;    //实时步频

uint16_t g_pace_avg         = 0;    //平均配速，单位sec/km

float step_amplitude_rt     = 0;    //实时步幅
uint8_t g_step_amplitude_avg   = 0; //平均步幅(单位cm)

float step_height_rt        = 0;    //实时垂直高度(单位m)  
float height_step_ratio_rt  = 0;    //实时垂直步幅比，每步垂直高度/步幅
float speed_allocation_rt   = 0;    //200米配速，单位(秒)

uint16_t    g_speed_avg     = 0;    //平均速度(实际速度放大10倍)
SPORT_DATA_T    g_sport_data_temp[2]                  = {0}; 
SPORT_DATA_T    g_sport_data_rt[SPORT_DATA_BUF_CNT]   = {0};    //实时运动数据，全程记录，每秒刷新一次，每30s存储一次
SPORT_DATA_T m_g_sport_data_rt;            //实时数据
uint8_t     g_sport_data_rt_index  = 0;    //实时运动数据数组下标

float m_dis_3sec_sum = 0;
float distance_last_3s[3]   = {0};  //最近3s距离数据，用于计算最近3s平均速度使用
uint8_t dis_last_3s_index   = 0;    //最近3s距离数据下标


static uint8_t step_zero_sec = 0;   //连续步数为0时长（无新增步数），单位：s
uint8_t isMove = 0;

static float GaussTemp[9] = {0};


static float g_acc_x[ACC_DATA_LEN_MAX] = {0};
static float g_acc_y[ACC_DATA_LEN_MAX] = {0};
static float g_acc_z[ACC_DATA_LEN_MAX] = {0};

static float aq3[ACC_DATA_LEN_MAX]      = {0};
static float y_filted[ACC_DATA_LEN_MAX] = {0};


//当前存储ACC数据总长度
static uint8_t g_u8acc_total_length = 0;

//acc数据首次达到3组
static bool first_3sec = false;

#if 0
//中间一组数据对应数组下标以及长度
static uint8_t g_u8acc_sec_group_index  = 0;
static uint8_t g_u8acc_sec_group_length = 0;
#endif

//*********************************/
//ACC数据计算波峰和波谷
/*********************************/

static uint8_t flagUpOrDown         = 0;                    //0代表下降，1代表上升
static uint8_t flagPeaksOrValley    = 0;                    //0代表上一个是波谷，1代表上一个是波峰

static float   MatrixPeaks[PEAKS_VALLEY_MAX]        = {0};   //记录波峰对应的acc数据，滤波处理后的数据
static uint8_t MatrixPeaksIndex[PEAKS_VALLEY_MAX]   = {0};   //记录波峰对应的acc数据索引
static uint8_t Peaks_cnt                            = 0;
static float   MatrixValley[PEAKS_VALLEY_MAX]       = {0};   //记录波谷对应的acc数据，滤波处理后的数据
static uint8_t MatrixValleyIndex[PEAKS_VALLEY_MAX]  = {0};   //记录波谷对应的acc数据索引
static uint8_t Valley_cnt                           = 0;


extern SPORTS_RT_DATA_MANAGER_T* pSports_rt_data;

extern uint32_t ble_data_send(uint8_t* p_data, uint16_t p_length);

//*********************************/
//
//*********************************/

float f_abs(float a)
{
    if (a >= 0)
    {
        return a;
    }
    else
    {
        return -a;
    }
}

double d_abs(double a)
{
    if (a >= 0)
    {
        return a;
    }
    else
    {
        return -a;
    }
}


//*****************************************************************************
// * 函 数 名  : Gauss_para_init
// * 函数功能  : 高斯滤波参数
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void Gauss_para_init(void)
{
    //高斯滤波参数
    for (uint8_t i = 0; i < 9; i++)
    {
        GaussTemp[i] = exp(-(i+1-5)*(i+1-5)/(2*0.49))/(0.7*sqrt(2*M_PI));
    }

}

//*****************************************************************************
// * 函 数 名  : KalmanFilterPara_init
// * 函数功能  : kalman滤波参数初始化
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void KalmanFilterPara_init(void)
{
    kfp_lat.A = 1;
    kfp_lat.H = 1;
    kfp_lat.Q = 0.09;
    kfp_lat.R = 16;
    kfp_lat.x = 0;
    kfp_lat.P = 16;
    kfp_lat.first = true;
    
    kfp_lon.A = 1;
    kfp_lon.H = 1;
    kfp_lon.Q = 0.09;
    kfp_lon.R = 16;
    kfp_lon.x = 0;
    kfp_lon.P = 16;
    kfp_lon.first = true;
}

//*****************************************************************************
// * 函 数 名  : SimpleKalman
// * 函数功能  : 一维kalman滤波处理
// * 输入参数  :	 
// *            z: 要处理的数据
// *            flag: 经纬度标志：0：纬度，1：经度
// * 输出参数  : 无
// * 返 回 值  : 滤波处理后的数据
//*****************************************************************************/
static double SimpleKalman(double z, bool flag)
{
    Kalman_Filter_Para_t * pKFP = NULL;

    if (flag)
    {
        pKFP = &kfp_lon;
    }
    else
    {
        pKFP = &kfp_lat;
    }

    if (pKFP->first)
    {
        pKFP->x = z;
        pKFP->first = 0;
    }

    //滤波算法处理
    double xp = pKFP->A * pKFP->x;
    double Pp = pKFP->A * pKFP->P * pKFP->A + pKFP->Q;

    double K = (Pp* pKFP->H)/(pKFP->H *Pp* pKFP->H + pKFP->R);

    pKFP->x = xp + K*(z - pKFP->H *xp);
    pKFP->P = Pp - K* pKFP->H *Pp;

    return pKFP->x;
}

//*****************************************************************************
// * 函 数 名  : algo_distance_gpscalculate
// * 函数功能  : 计算两个经纬度之间的距离
// * 输入参数  :	 
// *            lat1: 纬度1
// *            lng1: 经度1
// *            lat2: 纬度2
// *            lng2: 经度2
// * 输出参数  : 无
// * 返 回 值  : 距离
//*****************************************************************************/
static double algo_distance_gpscalculate(double lat1, double lng1, double lat2, double lng2)
{
    double   dst             = 0;
    double   radLat1         = 0;
    double   radLat2         = 0;
    double   a               = 0;
    double   b               = 0;
    
    //求弧度
    radLat1 = lat1*M_PI/180;
    radLat2 = lat2*M_PI/180;
    a = radLat1 - radLat2;
    b = (lng1 - lng2)*M_PI/180;
    
    //计算距离
    dst = 2 * asin((sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2) )));

    dst = dst * EARTH_RADIUS;
    if (dst < 0)
    {
        dst = -dst;
    }

    return dst;

}

//*****************************************************************************
// * 函 数 名  : algo_diatance_calculateall
// * 函数功能  : 批量计算距离
// * 输入参数  :	 
// *            plat: 纬度
// *            plng: 经度
// *            cnt:  经纬度组数
// * 输出参数  : 无
// * 返 回 值  : 距离
//*****************************************************************************/
static double algo_diatance_calculateall(double* plat, double* plng, uint8_t cnt)
{
    double distance = 0;

    //先滤波处理经纬度数据
    for (uint16_t i = 0; i < cnt; i++)
    {
        plat[i] = SimpleKalman(plat[i], 0);
        plng[i] = SimpleKalman(plng[i], 1);
    }

    //逐点计算距离
    for (uint8_t i = 0; i < (cnt - 1); i++)
    {
        distance += algo_distance_gpscalculate(plat[i], plng[i], plat[i + 1], plng[i + 1]);
    }

    return distance;
}


//*****************************************************************************
// * 函 数 名  : Acc_data_Gauss_filter
// * 函数功能  : 高斯滤波处理原始acc数据
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
static void Acc_data_Gauss_filter(void)
{
    uint8_t i = 0;

    uint8_t r = 5;
    
    //计算aq3
    for (i = 0; i < g_u8acc_total_length; i++)
    {
        aq3[i] = sqrt((g_acc_x[i] * g_acc_x[i]) + (g_acc_y[i] * g_acc_y[i]) + (g_acc_z[i] * g_acc_z[i]));
    }

    //高斯滤波处理
    for (i = 0; i < g_u8acc_total_length; i++)
    {
        y_filted[i] = aq3[i];
    }

    for (i = r; i <= (g_u8acc_total_length - r); i++)
    {
        y_filted[i] = 0;
        
        for (uint8_t j = 0; j < (2 * r - 1); j++)
        {
            y_filted[i] += aq3[i - r + 1 + j] * GaussTemp[j];
        }
//        NRF_LOG_INFO("y_filted[%d] = " NRF_LOG_FLOAT_MARKER "\r\n", i, NRF_LOG_FLOAT(y_filted[i]));
    }


}


//*****************************************************************************
// * 函 数 名  : Acc_data_Peaks_Valley_cal
// * 函数功能  : 计算分析波峰
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
static void Acc_data_Peaks_Valley_cal(void)
{
    Peaks_cnt = 0;
    Valley_cnt = 0;

    //根据前两个数据设置波形方向
    if (y_filted[1] >= y_filted[0])
    {
        flagUpOrDown = 1;
    }
    else
    {
        flagUpOrDown = 0;
    }

    //分析计算波形和波谷
    for (uint8_t i = 2; i < g_u8acc_total_length; i++)
    {
        //波形由上升转为下降
        if ((1 == flagUpOrDown) && (y_filted[i] < y_filted[i-1]))
        {
            flagUpOrDown = 0;
            
            if (Valley_cnt)
            {
                if ((y_filted[i-1] - MatrixValley[Valley_cnt-1]) < thd)
                {
                    continue;
                }
                else
                {
                    if (Peaks_cnt)
                    {
                        //两次波峰间隔要满足阈值要求，约200ms - 2s内
                        if(((i-1) - MatrixPeaksIndex[Peaks_cnt - 1] < INDEX_BETWEEN_MIN) && ((i-1) - MatrixPeaksIndex[Peaks_cnt - 1] > INDEX_BETWEEN_MAX))
                        {
                            continue;
                        }
                    }

                    //如果上一个是波峰，要特殊处理，是波谷就没有问题
                    if (1 == flagPeaksOrValley)
                    {
                        if(y_filted[i-1] > MatrixPeaks[Peaks_cnt-1])
                        {
                            //使用当前波峰替换上一个
                            MatrixPeaks[Peaks_cnt-1]        = 0;
                            MatrixPeaksIndex[Peaks_cnt-1]   = 0;
                            
                            Peaks_cnt--;
                        }
                        else
                        {
                            continue;
                        }
                    }
                    
                    //记录新的波峰
                    MatrixPeaks[Peaks_cnt]      = y_filted[i-1];
                    MatrixPeaksIndex[Peaks_cnt] = i - 1;
                    Peaks_cnt++;
                    
                    flagPeaksOrValley =1;
                                    
                }
            }
            //不可能两个波峰之间没有波谷, 但要考虑第一个最大值或者最小值，第一个波峰
            else
            {
                if (0 == Peaks_cnt)
                {
                    //第一个波峰
                    if (y_filted[i-1] > thd)
                    {
                        MatrixPeaks[0]      = y_filted[i-1];
                        MatrixPeaksIndex[0] = i - 1;
                        Peaks_cnt++;

                        flagPeaksOrValley   = 1;
                    }
                }
                else
                {
                    //需要更新使用当前的波峰为第一个波峰
                    if (y_filted[i-1] > MatrixPeaks[0])
                    {
                        MatrixPeaks[0]      = y_filted[i-1];
                        MatrixPeaksIndex[0] = i - 1;

                        flagPeaksOrValley   = 1;
                    }
                }
            }
        }
        //波形由下降转为上升
        else if ((0 == flagUpOrDown) && (y_filted[i] > y_filted[i-1]))
        {
            flagUpOrDown = 1;
            
            if (Peaks_cnt)
            {
                //没有达到阈值，不处理。
                if (f_abs(y_filted[i-1] - MatrixPeaks[Peaks_cnt-1]) < thd)
                {
                    continue;
                }
                else
                {
                    if (Valley_cnt)
                    {
                        if (((i-1) - MatrixValleyIndex[Valley_cnt - 1] < INDEX_BETWEEN_MIN) && ((i-1) - MatrixValleyIndex[Valley_cnt - 1] > INDEX_BETWEEN_MAX))
                        {
                            continue;
                        }
                    }

                    //如果上一个是波谷，要处理，是波峰就没有问题
                    if(flagPeaksOrValley == 0)
                    {
                        if (y_filted[i-1] < MatrixValley[Valley_cnt-1])
                        {
                            //使用当前波谷替换上一个
                            MatrixValley[Valley_cnt-1]      = 0;
                            MatrixValleyIndex[Valley_cnt-1] = 0;
                            
                            Valley_cnt--;
                        }
                        else
                        {
                            continue;
                        }
                    }

                    //记录新的波谷
                    MatrixValley[Valley_cnt]        = y_filted[i-1];
                    MatrixValleyIndex[Valley_cnt]   = i-1;
                    Valley_cnt++;
                    
                    flagPeaksOrValley =0;
                }
            }
            //第一次先出现波谷之前没有出现波峰，不处理
            else
            {
            }       
        }
    }

    if (0 == Valley_cnt)
    {
        Peaks_cnt = 0;
    }

}

//*****************************************************************************
// * 函 数 名  : data_handle_1hz
// * 函数功能  : 计算运动数据
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
static void data_handle_1hz(void)
{
     #if dist_test
        g_distance_total += 3;
     #endif

    float distance_second = g_distance_total - g_distance_last;     //本周期新增距离
    
    g_distance_last = g_distance_total;

    //处理本周期ACC数据，计算步数
    //高斯滤波处理
    Acc_data_Gauss_filter();
    
    //计算波峰/波谷
    Acc_data_Peaks_Valley_cal();

//    NRF_LOG_INFO("Peaks_cnt = %d", Peaks_cnt);
//    NRF_LOG_INFO("Peaks_cnt = %d", Valley_cnt);
    
    //本周期新增步数
    uint8_t stepnum_second = 0;
    
    //获取中间组ACC开始索引位置及结束索引位置
    #if 0
    uint8_t startindex1 = g_u8acc_sec_group_index;
    uint8_t endindex1   = g_u8acc_sec_group_index + g_u8acc_sec_group_length;
    #endif
    uint8_t startindex1 = FR_ACC;
    uint8_t endindex1   = FR_ACC + FR_ACC;

    
    //首次处理，需要计算前两组数据
    if (first_3sec)
    {
        startindex1 = 0;
    }
    
    uint8_t height_start_index = 0;
    uint8_t height_end_index   = 0;
    double  height_second      = 0; 
    uint8_t step_interval[5] = {0};
    uint8_t step_interval_cnt= 0;
    // 根据有效峰值点统计每秒步幅步频信息
    for (uint8_t ii = 0; ii < Peaks_cnt; ii++)
    {
        //计算高度变化
        // 每次脚掌落地时（ACC峰值点）假设垂直方向速度为0，垂直高度置0
        double v0       = 0;
        double height   = 0;
    
        //计算高度，从第一组的最后一个波峰开始，到第二组的最后一个波峰结束
        if (ii + 1 < Peaks_cnt)
        {
            //确定计算高度的开始index
            if((MatrixPeaksIndex[ii] < startindex1) && (MatrixPeaksIndex[ii + 1] >= startindex1))
            {
                height_start_index  = MatrixPeaksIndex[ii];
                height_end_index    = MatrixPeaksIndex[ii + 1];
                step_interval[step_interval_cnt++] = height_end_index - height_start_index;//每步时间间隔
            }
            else if ((MatrixPeaksIndex[ii] >= startindex1) && (MatrixPeaksIndex[ii + 1] < endindex1))
            {
                height_start_index  = MatrixPeaksIndex[ii];
                height_end_index    = MatrixPeaksIndex[ii + 1];
                step_interval[step_interval_cnt++] = height_end_index - height_start_index;//每步时间间隔
            }            
            
            for (uint8_t jj = height_start_index; jj < height_end_index; jj++)
            {
                // 假设垂直方向的加速度为aq1，减去重力加速度
                double acc_hight = g_acc_x[jj];
                double detalt    = 1/(double)FR_ACC;
                //NRF_LOF_INFO()
                double a0 = acc_hight - ONE_G;
                
                height += d_abs(v0*detalt + a0*(detalt*detalt)/2); // 计算每个采样间期的垂直运动距离
                
                // 更新速度信息
                v0 = v0 + a0 * detalt;  
            }
            height = height/2;  //在垂直方向存在上下运动，假设仅有一次来回

            //更新每秒高度
            height_second  += height; 
            //更新总高度
            g_height_total += height;
        }

        //只检测中间一组数据的波峰
        if ((MatrixPeaksIndex[ii] >= startindex1) && (MatrixPeaksIndex[ii] < endindex1))
        {
            stepnum_second++;//每秒步数累计
        }
        //第三组的数据不处理了
        else if (MatrixPeaksIndex[ii] >= endindex1)
        {
            break;
        }
        
    }

    //更新总步数
    #if step_test
        stepnum_second = 2;
    #endif
    g_step_total +=  stepnum_second; // 更新每秒步数
    		
		
    // 速度阈值判断，超过阈值保留前一秒的值
    if (distance_second > SPEED_MAX)
    {
        distance_second = SPEED_MAX;
    }

    //实时步频、步幅
    if (stepnum_second)
    {
        step_zero_sec = 0;
        if(isMove == 0)
        {
            isMove = 1;
        }
        
        #if height_test
            height_second = 0.16;
        #endif
        
        float step_height = height_second/stepnum_second;  //计算每秒垂直高度
        //垂直高度判断，超过阈值保留前一秒的值
        if(step_height <= STEP_HEIGHT_MAX)
        {
            step_height_rt  = step_height;
        }
   
        float step_amplitude = distance_second/stepnum_second;  // 计算每秒步幅

        // 步幅阈值判断，超过阈值保留前一秒的值
        if(step_amplitude <= STEP_AMPLITUDE_MAX)
        {
            step_amplitude_rt = step_amplitude;
        }

        float height_step_ratio = step_height_rt/step_amplitude_rt;//计算实时垂直步幅比

        if(height_step_ratio <= HEIGHT_STEP_RATIO)
        {
              height_step_ratio_rt = height_step_ratio;
        }
        
        //NRF_LOG_INFO("step_amplitude_rt=%d",step_amplitude_rt*1000);
        //NRF_LOG_INFO("step_height_rt=%d",step_height_rt*1000);
         
        float step_interval_avg = 0;
        for(uint8_t m =0; m < step_interval_cnt; m++)
        {
            if(step_interval[m] != 0)
            {
                step_interval_avg += ((float)(FR_ACC)/step_interval[m]);    
            }
        }
        step_interval_avg = step_interval_avg/(stepnum_second);
        
        #if step_freq_test
            step_interval_avg = 2; 
        #endif
        
        uint16_t step_frequency = step_interval_avg * 60; // 计算步频信息
        
        // 步频阈值判断
        if(step_frequency <= STEP_FREQUENCY_MAX)
        {
            step_freq_rt = step_frequency;
        }
    }
    else
    {
        step_zero_sec++;    //无新增步数时长增加
        
        // 步数为0持续时间小于3秒，沿用上一秒步数信息， 大于等于3秒，步频、垂直高度、步幅置0
        if(step_zero_sec >= 3) 
        {
            step_freq_rt = 0; 
            step_amplitude_rt = 0;
            step_height_rt = 0;
            height_step_ratio_rt = 0;
            isMove = 0;
        }
    }

    // 实时速度（最近3秒平均）
    float dis_3sec_sum = 0;

    //先更新最近3次的距离信息
    if (dis_last_3s_index >= 3)
    {
        distance_last_3s[0] = distance_last_3s[1];
        distance_last_3s[1] = distance_last_3s[2];
        distance_last_3s[2] = distance_second;
    }
    else
    {
        distance_last_3s[dis_last_3s_index++] = distance_second;
    }

    //根据最近几次的距离信息计算速度
    for (uint8_t i = 0; i < dis_last_3s_index; i++)
    {
        dis_3sec_sum += distance_last_3s[i];
    }
    
    m_dis_3sec_sum = dis_3sec_sum;

    //总平均步频、步幅、高度、速度等计算
    if (g_step_total)
    {
        g_step_amplitude_avg    = (g_distance_total*100 / g_step_total);
    }
    
    if (g_sport_data.u32time_Sec)
    {
        g_step_freq_avg = g_step_total / g_sport_data.u32time_Sec;
        g_speed_avg     = (g_distance_total / g_sport_data.u32time_Sec)*10;

        g_pace_avg      = (g_sport_data.u32time_Sec *1000) / g_distance_total;
    }   
    first_3sec = false; 
}


uint8_t sport_data_index = 0;
void save_flash_data()
{
    //先更新最近3次的距离信息
    if (sport_data_index >= 2)
    {
        memcpy(&g_sport_data_temp[0], &g_sport_data_temp[1], sizeof(SPORT_DATA_T));
        
        g_sport_data_temp[1].u8heart      = g_heart_value;
        g_sport_data_temp[1].u16distance_total    = (uint16_t)g_distance_total;
        g_sport_data_temp[1].u16step_total        = g_step_total;
        g_sport_data_temp[1].u16cal               = g_cal_total;
        g_sport_data_temp[1].u8speed_rt           = (m_dis_3sec_sum / dis_last_3s_index)*10;
        g_sport_data_temp[1].u16step_fre_rt       = step_freq_rt;
        g_sport_data_temp[1].u8step_amplitude_rt  = step_amplitude_rt*100;

    }
    else
    {
        g_sport_data_temp[sport_data_index].u8heart      = g_heart_value;
        g_sport_data_temp[sport_data_index].u16distance_total    = (uint16_t)g_distance_total;
        g_sport_data_temp[sport_data_index].u16step_total        = g_step_total;
        g_sport_data_temp[sport_data_index].u16cal               = g_cal_total;
        g_sport_data_temp[sport_data_index].u8speed_rt           = (m_dis_3sec_sum / dis_last_3s_index)*10;
        g_sport_data_temp[sport_data_index].u16step_fre_rt       = step_freq_rt;
        g_sport_data_temp[sport_data_index].u8step_amplitude_rt  = step_amplitude_rt*100;
        
//        g_sport_data_temp[sport_data_index].u8speed_rt   = (m_dis_3sec_sum / dis_last_3s_index)*10;
//        g_sport_data_temp[sport_data_index].u8heart      = g_heart_value;
//                
//        g_sport_data_temp[sport_data_index].u8sport_state        = m_bracelet_state.train_state;
//        g_sport_data_temp[sport_data_index].u8sport_id           = m_bracelet_state.train_id;
//        g_sport_data_temp[sport_data_index].u8sport_level        = m_bracelet_state.train_level;
//        g_sport_data_temp[sport_data_index].u16sport_group       = m_bracelet_state.train_num_OneStep;
//        g_sport_data_temp[sport_data_index].u8sport_proce_total  = m_bracelet_state.train_all_step;
//        g_sport_data_temp[sport_data_index].u8sport_proce_cur    = m_bracelet_state.train_cur_step;
//        g_sport_data_temp[sport_data_index].u16step_total        = g_step_total;
//        g_sport_data_temp[sport_data_index].u16cal               = g_cal_total;
//        g_sport_data_temp[sport_data_index].u8step_fre_avg       = g_step_freq_avg;
//        g_sport_data_temp[sport_data_index].u16height_total      = (uint16_t)(g_height_total*100);
//        g_sport_data_temp[sport_data_index].u16distance_total    = (uint16_t)g_distance_total;
//        g_sport_data_temp[sport_data_index].u8step_amplitude_avg = g_step_amplitude_avg;
//        g_sport_data_temp[sport_data_index].u8speed_avg          = g_speed_avg;
//              
//        g_sport_data_temp[sport_data_index].u16step_fre_rt       = step_freq_rt;
//        g_sport_data_temp[sport_data_index].u16timer_run_score   = timer_run_score;
//        g_sport_data_temp[sport_data_index].u16height_step_ratio = height_step_ratio_rt*1000;
//        g_sport_data_temp[sport_data_index].u16step_train        = g_train_step;
//        g_sport_data_temp[sport_data_index].u16distance_train    = g_train_distance;
//        g_sport_data_temp[sport_data_index].u8step_amplitude_rt  = step_amplitude_rt*100;

        sport_data_index++;
    }
    
    #if 1
    m_g_sport_data_rt = g_sport_data_temp[1];//实时数据
    //发送BLE实时数据
    prepare_ble_uart_data();
    ble_data_send((uint8_t*)&g_ble_uart_buf, g_ble_uart_buf_size);
    #endif
    
    if(save_data_flag)
    {
        memcpy(&g_sport_data_rt[g_sport_data_rt_index], &g_sport_data_temp[0], sizeof(SPORT_DATA_T));
        g_sport_data_rt_index++;

        //达到30次，存flash处理
        if (g_sport_data_rt_index >= SPORT_DATA_BUF_CNT)
        {
            g_sport_data.sports_rt_info.u16speed_rt_cnt += g_sport_data_rt_index;
            sports_data_rt_save_flash();
            g_sport_data_rt_index = 0;
        }    
    }
}

//*****************************************************************************
// * 函 数 名  : data_collect_Acc
// * 函数功能  : 周期处理6轴sensor上传过来的acc数据；处理原则，保留最近3个周期的数据，最新数据放最后
// * 输入参数  :	 
// *            u16acc_x: 
// *            u16acc_y: 
// *            u16acc_z: 
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void data_collect_Acc(int16_t i16acc_x, int16_t i16acc_y, int16_t i16acc_z)
{
    float xxx = i16acc_x;
    float yyy = i16acc_y;
    float zzz = i16acc_z;
    
    if (g_u8acc_total_length < ACC_DATA_LEN_MAX)
    {
        g_acc_x[g_u8acc_total_length] = (ONE_G*xxx)/SSVT_A;
        g_acc_y[g_u8acc_total_length] = (ONE_G*yyy)/SSVT_A;
        g_acc_z[g_u8acc_total_length] = (ONE_G*zzz)/SSVT_A;
        g_u8acc_total_length++;
    }
    
    //有3组数据，调用算法计算
    if (ACC_DATA_LEN_MAX == g_u8acc_total_length)
    {
        data_handle_1hz();

        //已经满3组数据，移除掉最早的一组数据
        for (uint8_t i = FR_ACC; i < ACC_DATA_LEN_MAX; i++)
        {
            g_acc_x[i - FR_ACC] = g_acc_x[i];
            g_acc_y[i - FR_ACC] = g_acc_y[i];
            g_acc_z[i - FR_ACC] = g_acc_z[i];
        }
        
        //更新第二组信息
        g_u8acc_total_length = ACC_DATA_LEN_MAX - FR_ACC;
    }
    
}

//*****************************************************************************
// * 函 数 名  : data_collect_GPS
// * 函数功能  : 周期处理GPS上传过来的经纬度数据；处理原则，为方便和上一组数据衔接，把上一组数据的最后一条放置到本组第一条之前
// * 输入参数  :	 
// *            pu16GPS_Lat: 
// *            pu16GPS_Lon: 
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void data_collect_GPS(double GPS_Lat, double GPS_Lon)
{
    g_GPS_lat[g_GPS_data_cnt] = GPS_Lat;
    g_GPS_lon[g_GPS_data_cnt] = GPS_Lon;
    g_GPS_data_cnt++;

    //按照一组处理一次，此处采样率设置为10hz，实际可能有偏差
    if (g_GPS_data_cnt >= GPS_buf_len)
    {
        //计算本组数据位置
        g_distance_total += algo_diatance_calculateall(g_GPS_lat, g_GPS_lon, g_GPS_data_cnt);

        //用完之后保留上组的最后一条数据，以便和下次的数据衔接起来
        g_GPS_lat[0] = g_GPS_lat[g_GPS_data_cnt - 1];
        g_GPS_lon[0] = g_GPS_lon[g_GPS_data_cnt - 1];
        g_GPS_data_cnt = 1;
    }

}

//*****************************************************************************
// * 函 数 名  : sport_data_init
// * 函数功能  : 开始运动，设置标志位，初始化参数数据
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void sport_data_init(void)
{
    KalmanFilterPara_init();

    g_GPS_data_cnt      = 0;
    g_distance_total    = 0;
    g_distance_last     = 0;
    g_step_total        = 0;
    g_cal_total         = 0;
    g_height_total      = 0;

    first_3sec          = false;

    step_zero_sec       = 0;
    
    step_amplitude_rt   = 0;
    g_step_amplitude_avg = 0;
    step_freq_rt        = 0;
    g_step_freq_avg     = 0;
    g_speed_avg         = 0;
    g_pace_avg          = 0;

    memset(g_sport_data_rt, 0, sizeof(g_sport_data_rt));
    g_sport_data_rt_index      = 0;

    distance_last_3s[0] = 0;
    distance_last_3s[1] = 0;
    distance_last_3s[2] = 0;
    dis_last_3s_index   = 0;
    
    g_u8acc_total_length    = 0;

    flagUpOrDown         = 0;
    flagPeaksOrValley    = 0;
    Peaks_cnt            = 0;
    Valley_cnt           = 0;

    
    //记录本次运动实时速度的存储位置
    g_sport_data.sports_rt_info.u16speed_rt_cnt  = 0;
    g_sport_data.sports_rt_info.u32addr_start    = g_his_data_manager_info.rt_data.cur_offset;
}

//*****************************************************************************
// * 函 数 名  : sport_data_end
// * 函数功能  : 结束运动，处理运动数据
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void sport_data_end(void)
{
    //记录本次运动实时速度的存储位置和数量
    if (g_sport_data_rt_index)
    {
        g_sport_data.sports_rt_info.u16speed_rt_cnt += g_sport_data_rt_index;
        sports_data_rt_save_flash();
        g_sport_data_rt_index = 0;
    }

    //记录本次心率的存储位置
    g_sport_data.sports_rt_info.u32addr_end = pSports_rt_data->cur_offset;

    sport_data_sum_save_flash();

    //更新历史数据管理信息
    g_his_data_manager_info.u8his_cnt++;
}


